#include "imu.h"
IMU::IMU()
{
	Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL);
	Wire.setClock(400000);
	while (!imu.testConnection());
	imu.initialize();

	imu.setXAccelOffset(0);
	imu.setYAccelOffset(0);
	imu.setZAccelOffset(0);
	imu.setXGyroOffset(0);
	imu.setYGyroOffset(0);
	imu.setZGyroOffset(0);
	imu.CalibrateAccel(5);
}

Action IMU::update(int interval)
{
	action = NOACT;
	if (millis() - last_update_time > interval)
	{
		imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
		
		if (ay > 10000 && is_valid)
		{
			is_valid = true;
			action = TURN_LEFT;
		}
		else if (ay < -10000 && is_valid)
		{
			is_valid = true;
			action = TURN_RIGHT;
		}
		else
		{
			is_valid = false;
		}

		if (ax > 7000 && !is_valid)
        {
            delay(200);
            imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
            if (ax > 7000)
            {
                is_valid = true;
                action = FORWARD;
            }
        }
        else if (ax < -10000 && !is_valid)
        {
            delay(200);
            imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
            if (ax < -10000)
            {
                is_valid = true;
                action = BACK;
            }
        }
        else
        {
            is_valid = true;
        }
		last_update_time = millis();
	}
	return action;
		
}

int16_t IMU::getAccelX()
{
	return ax;
}

int16_t IMU::getAccelY()
{
	return ay;
}

int16_t IMU::getAccelZ()
{
	return az;
}

int16_t IMU::getGyroX()
{
	return gx;
}

int16_t IMU::getGyroY()
{
	return gy;
}

int16_t IMU::getGyroZ()
{
	return gz;
}
